/*
 * drive_control.c
 *
 *  Created on: 2012.12.10.
 *      Author: Peter
 */
#include "stm32f4xx.h"
#include "drive.h"

#define DRV_INT_MAX 800

float drv_int_err = 0;
float tst_speed;

void Drive_Control(int16_t req_speeds, int16_t speed)
{
	int16_t drive_pwm=0;
	float error;

	// P
	if(req_speeds == 100)
	{
		error = -speed;
	}
	else
	{
		error = req_speeds-speed;
	}
	// I
	drv_int_err += error;
	if(drv_int_err > DRV_INT_MAX) drv_int_err = DRV_INT_MAX;
	if(drv_int_err < -DRV_INT_MAX) drv_int_err = -DRV_INT_MAX;

	drive_pwm = (int16_t)(20*error + 0.5*drv_int_err); //P:10,0,0.5
	if(req_speeds > 0)
	{
		drive_pwm += 200;
	} else if(req_speeds < 0)
	{
		drive_pwm -= 200;
	}
	if(drive_pwm>1050) drive_pwm = 1050;
	else if(drive_pwm < -1050)drive_pwm = -1050;

	SetDrivePWM(drive_pwm);
}

void Drive_ControlFast(int16_t req_speeds, int16_t speed)
{
	int16_t drive_pwm=0;
	float error;

	// P
	if(req_speeds == 100)
	{
		error = -speed;
	}
	else
	{
		error = req_speeds-speed;
	}
	// I
	drv_int_err += error;
	if(drv_int_err > DRV_INT_MAX) drv_int_err = DRV_INT_MAX;
	if(drv_int_err < -DRV_INT_MAX) drv_int_err = -DRV_INT_MAX;

	drive_pwm = (int16_t)(20*error + 0.5*drv_int_err); //P:10,0,0.5
	if(req_speeds > 0)
	{
		drive_pwm += 350;
	} else if(req_speeds < 0)
	{
		drive_pwm -= 350;
	}
	if(drive_pwm>1050) drive_pwm = 1050;
	else if(drive_pwm < -1050)drive_pwm = -1050;
	tst_speed = drv_int_err;
	SetDrivePWM(drive_pwm);
}
